import rclpy  
from rclpy.node import Node  
from nav_msgs.msg import Odometry   
from geometry_msgs.msg import Twist  
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import Imu  
import numpy as np  
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Int32
from rcl_interfaces.msg import SetParametersResult
from ai_msgs.msg import PerceptionTargets
from origincar_msg.msg import Sign  # 根据图片的消息类型
from origincar_msg.msg import Data
import math
import datetime
import time
from pyzbar.pyzbar import decode

from sensor_msgs.msg import Image
import cv2

from threading import Thread
import keyboard

import sys
import tty
import termios

class InertialNavigationNode(Node):  
    def __init__(self):  
        super().__init__('navigation_node')  # 初始化节点名称
        # 用于判断是否允许开始导航，1为开始，0为停止
        self.flag = 1
        #用于设置状态，0为简单导航，1为避障导航，2为二维码导航，3为停车点导航  
        self.state = 0
        #目标点切换（任务一任务三启动标志）
        self.task = 0
        #任务中途切换目标位置

        # 创建IMU数据订阅
        self.subscription_imu_data = self.create_subscription(  
            Imu,  
            '/imu/data_raw',  
            self.imu_callback,  
            3
        )
        # 创建目标位置订阅，主要用于订阅目标位置，这个的y的位置偏差较大
        self.subscription_robotvel = self.create_subscription(
            Data,  
            '/robotvel',
            self.robotvel_callback,
            3
        )

        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10)     
        
        self.subscription = self.create_subscription(
            Image,
            '/image',
            self.image_callback,
            3)


        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)  
        # 创建发布者，发布到 /sign_switch 话题
        self.sign_switch_pub = self.create_publisher(Sign, 'sign_switch', 10)
        #创建新的发布者，用于发布park信息
        self.park_publisher = self.create_publisher(Int32MultiArray, '/park', 10)  # 创建新的发布者，用于发布x和y坐标   

        #这里先初始化一下任务一的路径
        self.path_task1 = [
            (2.5, 1.0), 
            (3.8, 1.0),
            (4.5,1.6),
        ]
        self.current_target_idx_task1 = 0  # 任务一当前目标点索引
        #这里先初始化一下任务三的路径
        self.path_task3 = [
            (2.0,1.5),
            (1.2,1.0),
            (0.3,0.2)
        ]
        self.current_target_idx_task3 = 0  # 任务三当前目标点索引


        #图像识别参数
        self.area = 1200    
        self.right_park = 640
        self.up_park = 270    


        # 初始化状态变量
        self.position = np.array([0.0, 0.0])  # 当前位置信息
        self.angular =  0.0  # 当前角度信息

        #目标地点
        self.target_position = np.array([0.0, 0.0])         
     
        
        #由于识别到停车场和二维码后会前往，所以这里设置了相对位置
        self.relativ_target_position = np.array([0.0, 0.0])  # 相对目标位置
        self.park_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        


        #这里使用了加速度计校准
        self.sample_size =100 #误差采样个数
        self.angular_velocity_sample = []#放置误差的地方
        self.bias_correction = 0#平均误差
        self.imu_z = 0 #imu的z轴数据
        self.imu_last_time = self.get_clock().now()  

        #初始化task_time开始时间 
        self.task3_time = 0



        #参数设置
        
        self.declare_parameter ('stop_distance',0.1) 
        self.declare_parameter ('vx',0.7)
        self.declare_parameter ('vx_3',0.25)
        self.declare_parameter ('safe_distance',0.8) 
        self.declare_parameter ('turn_time_1',1.0)
        self.declare_parameter ('turn_time_2',1.2)
        self.declare_parameter ('turn_v',0.4)
        self.declare_parameter ('turn_w',0.01)
        self.declare_parameter ('turn_error',0)
        self.declare_parameter ('zhuitongyuzhi',320) 
        self.declare_parameter ('radius_cone',0.3)


        #到达目标点的距离阈值
        self.stop_distance = self.get_parameter("stop_distance").value

        self.vx = self.get_parameter("vx").value
        self.vx_3 = self.get_parameter("vx_3").value
        self.safe_distance = self.get_parameter("safe_distance").value
        self.turn_time_1 = self.get_parameter("turn_time_1").value
        self.turn_time_2 = self.get_parameter("turn_time_2").value
        self.turn_v = self.get_parameter("turn_v").value
        self.turn_w = self.get_parameter("turn_w").value   
        self.turn_error = self.get_parameter("turn_error").value  
        self.zhuitongyuzhi = self.get_parameter("zhuitongyuzhi").value
        self.radius_cone = self.get_parameter("radius_cone").value

        #参数回调函数
        self.add_on_set_parameters_callback(self.parameters_callback)


    '''
    task1的部署,依次前往设定的目标点
    '''
    def task1(self):
        # 所有目标点已完成
        if self.current_target_idx_task1  >= len(self.path_task1):
            return True
        
        target_x, target_y = self.path_task1[self.current_target_idx_task1]

        dx = target_x - self.position[0]
        dy = target_y - self.position[1]
        distance = np.hypot(dx, dy)

        
        # 到达当前目标点，切换下一个
        if distance < self.safe_distance:
            self.current_target_idx_task1 += 1
            return False
        
        # 计算目标方向角度
        target_angle = np.arctan2(dy, dx)
        angle_error = target_angle - self.angular
        # 构造控制指令  
        cmd_msg = Twist() 
        cmd_msg.linear.x = self.vx
        cmd_msg.angular.z = 2 * angle_error  # 设置角速度  
            
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令 
    
    '''
    task3的部署,依次前往设定的目标点
    '''
    def task3(self):
        if self.current_target_idx_task3 >= len(self.path_task3):
            return True
        
        target_x, target_y = self.path_task3[self.current_target_idx_task3]

        dx = target_x - self.position[0]
        dy = target_y - self.position[1]
        distance = np.hypot(dx, dy)

        #识别停车点
        if self.state == 3 :
            self.park_navigation()
        
        # 到达当前目标点，切换下一个
        if distance < self.safe_distance:
            self.current_target_idx_task3 += 1
            return False
        
        # 计算目标方向角度
        target_angle = np.arctan2(dy, dx)
        angle_error = target_angle - self.angular
        # 构造控制指令  
        cmd_msg = Twist() 
        cmd_msg.linear.x = self.vx
        cmd_msg.angular.z = 2 * angle_error  # 设置角速度  
            
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令 
 
    def image_callback(self, msg):
        np_arr = np.frombuffer(msg.data, np.uint8)
        cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        # 二维码识别
        if(self.task == 1):
            self.decode_qr_code_pyzbar(cv_image)  
        #停车场识别
        if(self.task == 3 and (self.get_clock().now() - self.task3_time).nanoseconds * 1e-9 > 3):
            self.park_relativ_position = self.park_frame(cv_image)

            #发布park坐标
            park_msg = Int32MultiArray()
            park_msg.data = self.park_relativ_position
            self.park_publisher.publish(park_msg)

            if(self.park_relativ_position[0] != 0):
                self.state = 3  #设置状态
            else:
                self.state = 0
           
    def park_navigation(self):
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx_3  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度
        
        self.cmd_pub.publish(cmd_msg)  
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx_3  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度
        self.get_logger().info('识别到park')

        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

    def sign4return_callback(self,msg):
    # 创建 Sign 消息实例
        if msg.data == 6:
            self.flag = 1
            self.state = 0
            self.task = 3

            # 定位任务三中途目标点
            self.target_position[0] = -0.5  # 目标x和y坐标
            self.target_position[1] = 0 
            self.get_logger().info(f"任务三")
            self.get_logger().info(f"结束遥感")
            #获取当前时间
            self.task3_time = self.get_clock().now()

            self.position[0] = 2.5
            self.position[1] = 2.2
            ##这里会重置里程计到(2.5,2.2)

        if msg.data == -1:
            self.flag = 1
            self.state = 0
            self.task = 1
            #前往二维码区
       
            self.get_logger().info(f"任务一")
            self.get_logger().info(f"重置里程计")
 
            self.angular = 0    
            self.position[0] = 0.5
            self.position[1] = 0.2
            ##这里重置了里程计，将他的位置初始化成（0.5，0.2）
            
        if msg.data == 5:
            self.flag = 0
            self.task = 2
            self.get_logger().info(f"任务二")
            self.get_logger().info(f"C区进行遥测")

        return
        

    def imu_callback(self, msg):  
        #更新IMU数据
        #角速度
        angular_velocity = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])  
        if len(self.angular_velocity_sample) <= self.sample_size:
            self.angular_velocity_sample.append(angular_velocity[2])   
        elif len(self.angular_velocity_sample) == self.sample_size:
            # 计算偏移量
            self.bias_correction = np.mean(self.angular_velocity_sample)
            self.get_logger().info('偏移量: {}'.format(self.bias_correction))
        elif len(self.angular_velocity_sample) > self.sample_size: 
             self.imu_z = angular_velocity[2]-self.bias_correction

        if self.task == 1 :
            self.task1()
        elif self.task == 3 :
            self.task3()
            self.get_logger().info('任务三目标位置位置: x={:.6f}, y={:.6f},state={}'.format(self.target_position[0], self.target_position[1],self.state))

        

    def robotvel_callback(self , msg):
        x_speed = msg.x
        current_time = self.get_clock().now()
        dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
        self.imu_last_time = current_time

        angular = self.angular
        self.position[0]+=1.13*( x_speed * math.cos(angular) ) * dt
        self.position[1]+=1.03*( x_speed * math.sin(angular) ) * dt
        
        if (x_speed != 0):
            self.angular = self.imu_z * dt + self.angular
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    
        self.get_logger().info('x={:.2f}, y={:.2f}, 角度={:.3f},状态{},'.format(self.position[0], self.position[1], self.angular,self.state))



    def decode_qr_code_pyzbar(self, image):
        # 使用 pyzbar 进行二维码解码
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        decoded_objects = decode(gray)
        
        # 遍历结果并判断是否是顺时针或逆时针二维码
        for obj in decoded_objects:
            qr_code_data = obj.data.decode('utf-8')
            if qr_code_data in ["ClockWise", "AntiClockWise"]:
                self.publish_sign_message(qr_code_data)
                self.flag  = 0 
                self.task = 2
          
                twist_msg = Twist()
                twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
                twist_msg.angular.z = 0.00  # 使用 角速度复位
                # 发布 Twist 消息
                self.cmd_pub.publish(twist_msg)
                self.get_logger().info(f"已复位")
                return  # 成功识别并发布后立即返回，避免重复处理
    
    
    def publish_sign_message(self, qr_code_data):
        # 创建并发布 Sign 消息
        sign_msg = Sign()
        sign_msg.sign_data = 3 if qr_code_data == "ClockWise" else 4
        self.sign_switch_pub.publish(sign_msg)
        self.get_logger().info(f"已发布二维码信息: {qr_code_data}")


    def park_frame(self,frame):
            x0 = 0
            y0 = 0
            w0 = 0
            h0 = 0
            im0 = frame[self.up_park:450, 0:self.right_park]#增大右边界就要减小lower_hsv2第二个阈值
            hsv_img = cv2.cvtColor(im0, cv2.COLOR_BGR2HSV)

            # Define range of target color in HSV
            lower_hsv1 = np.array([0, 0, 0])
            lower_hsv2 = np.array([47, 73, 255])
            upper_hsv1 = np.array([0, 0, 0])
            upper_hsv2 = np.array([255, 50, 255])#60

            # Threshold the HSV image，阈值内为白色（255）
            imgThreshLow = cv2.inRange(hsv_img, lower_hsv1, lower_hsv2)
            imgThreshHigh = cv2.inRange(hsv_img, upper_hsv1, upper_hsv2)
            threshed_img = cv2.bitwise_and(imgThreshLow, 255-imgThreshHigh)

            # Find contours in the black regions of smoothed_img
            contours, hierarchy = cv2.findContours(threshed_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

            # Initialize variables to store the largest contour
            max_area = 0
            largest_rect = None
            largest_cnt = None

            # Identify the largest contour and store its bounding rectangle
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > self.area:  # Filter small contours if needed
                    x, y, w, h = cv2.boundingRect(cnt)
                    if area > max_area:
                        max_area = area
                        largest_rect = (x, y, w, h)
                        largest_cnt = cnt

            # Draw bounding rectangles and centroids
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > self.area:
                    x, y, w, h = cv2.boundingRect(cnt)

                    # Draw the largest rectangle in red, others in green
                    if (x, y, w, h) == largest_rect:

                        x0 = int(x + w / 2)
                        y0 = int(y + self.up_park + h / 2)
                        w0 = int(w)
                        h0 = int(h)
                
            return  x0,y0,w0,h0 


    def parameters_callback(self,paramters):
        for paramter in paramters:
            self.get_logger().info("Parameter: {} - Value: {}".format(paramter.name, paramter.value))
            if paramter.name == "vx":
                self.vx = paramter.value   
            elif paramter.name == "vx_3":
                self.vx_3 = paramter.value   
            elif paramter.name == "stop_distance":
                self.stop_distance = paramter.value  
            elif paramter.name == "safe_distance":
                self.safe_distance = paramter.value
            elif paramter.name == "turn_time_1":
                self.turn_time_1 = paramter.value  
            elif paramter.name == "turn_time_2":
                self.turn_time_2 = paramter.value
            elif paramter.name == "turn_v":
                self.turn_v = paramter.value  
            elif paramter.name == "turn_w":
                self.turn_w = paramter.value 
            elif paramter.name == "turn_error":
                self.error = paramter.value    
            elif paramter.name == "zhuitongyuzhi":
                self.zhuitongyuzhi = paramter.value  
            elif paramter.name == "radius_cone":
                self.radius_cone = paramter.value  
     
        return SetParametersResult(successful=True)


def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation = InertialNavigationNode()  #
    rclpy.spin(navigation)
    navigation.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数



